#include "gongxun.h"
#include "MotorControl.h"
#include "uart234.h"
#include "ControlTask.h"
#include "lineFollowSensor.h"
#include "PID.h"
#include "user_usart.h"
#include <stdio.h>
#include "main.h"

Meterial meterial[6];
uint8_t queue[6];


/*
start point 0,
erweima 1,
raw material area 2,
rough machining area 3,
Semi-finished products area 4,
return point 5,
*/
void OnTheWay(unsigned int vectorFrom,unsigned int vectorTo)
{
	unsigned int vector = vectorFrom*10+vectorTo;
	switch(vector)
	{
		case 01:
			/* have some problems*/
			
			break;
		case 12:
			OneGrid(LEFT,0);
			for(int i=0;i<3;i++)
			{
				OneGrid(FRONT,0);
			}
			//OneGrid(0,-0.6*userSpeed );
			break;
		case 23:
			for(int i=0;i<2;i++)
			{
				OneGrid(LEFT,0);
			}
			break;
		case 32:
			for(int i=0;i<2;i++)
			{
				OneGrid(RIGHT,0);
			}
			break;
		case 34:
			for(int i=0;i<2;i++)
			{
				OneGrid(BACK,0);
			}
			for(int i=0;i<2;i++)
			{
				OneGrid(LEFT,0);
			}
			break;
		case 43:
			for(int i=0;i<2;i++)
			{
				OneGrid(RIGHT,0);
			}
			for(int i=0;i<2;i++)
			{
				OneGrid(FRONT,0);
			}
			break;
		case 42:
			for(int i=0;i<4;i++)
			{
				OneGrid(RIGHT,0);
			}
			for(int i=0;i<2;i++)
			{
				OneGrid(FRONT,0);
			}
		case 45:
			/*have some problems*/
			break;
		
	}
	return;
}

void ManufacturingProcesses()
{
	unsigned int index=0;
	/*data preparation*/
	for(;index<6;index++)
		{
			meterial[index].itsColor=Color[index];
		}
	for(index=0;index<3;index++)
		{
			meterial[index].itsOrder=Max7219_String[index];
		}
	for(index=5;index<8;index++)
		{
			meterial[index].itsOrder=Max7219_String[index]+3;
		}
	for(index=0;index<6;index++)
		{
			queue[meterial[index].itsOrder]=index;
		}
	/*performing*/
	for(index=0;index<2;index++)
		{
			for(unsigned int i=0;i<3;i++)
				{ 
					Uart2_servoCtr(2);//raising the paw
					HAL_Delay(2000);
					Uart2_servoCtr(2+meterial[queue[i+index]].itsOrder);//aiming
//					Uart2_servoCtr(20);//grabing
//					Uart2_servoCtr(16);//ready to go
					OnTheWay(2,3);
					if(meterial[queue[i+index]].itsColor=='B')
						Uart2_servoCtr(9);//aiming
					else if(meterial[queue[i+index]].itsColor=='R')
						Uart2_servoCtr(10);//aiming
					else if(meterial[queue[i+index]].itsColor=='G')
						Uart2_servoCtr(11);//aiming
//					Uart2_servoCtr(21);//put
					Uart2_servoCtr(1);//ready to back
					if(i!=2)
						{
							OnTheWay(3,2);
						}
				}
			for(unsigned int i=0;i<3;i++)
				{
					Uart2_servoCtr(2);//raising the paw
					if(meterial[queue[i+index]].itsColor=='B')
						Uart2_servoCtr(9);//aiming
					else if(meterial[queue[i+index]].itsColor=='R')
						Uart2_servoCtr(10);//aiming
					else if(meterial[queue[i+index]].itsColor=='G')
						Uart2_servoCtr(11);//aiming
//					Uart2_servoCtr(20);//grabing
					Uart2_servoCtr(16);//ready to go
					OnTheWay(3,4);
					if(meterial[queue[i+index]].itsColor=='B')
						Uart2_servoCtr(12+3*index);//aiming
					else if(meterial[queue[i+index]].itsColor=='R')
						Uart2_servoCtr(13+3*index);//aiming
					else if(meterial[queue[i+index]].itsColor=='G')
						Uart2_servoCtr(14+3*index);//aiming
//					Uart2_servoCtr(21);//put
					Uart2_servoCtr(1);//ready to back
					if(i!=2)
						{
							OnTheWay(4,3);
						}
				}
			if(index==1)
				{
					OnTheWay(4,5);//over
				}
			else 
				OnTheWay(4,2);
		}
}
